/*
* (c) 2014 University of Applied Sciences, Karlsruhe
* Project "Segmentation of depth data of a plenoptic camera"
* summer semester 2014
*
* transformation_3d.h
* This file contains helper functions which are used to transform point or point clouds between different coordinate systems. Additionally, methods
* to projects points from and to camera space or the image plane are also stored in this file.
*/


#ifndef _TRANSFORMATION_3D_H_
#define _TRANSFORMATION_3D_H_

#include <cassert>
#include <math.h>
#include <vector>

#include <pcl/point_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/common/distances.h>

#include <opencv2/opencv.hpp>
#include <Eigen/Dense>


#include "sgBase/camera_3d.h"
#include "sgBase/visualization_3d.h"

using namespace Eigen;

namespace sgMain
{
	namespace Transformation3D
	{
		/**
		Transforms a point from camera space into world coordinates.
		*/
		pcl::PointXYZRGB TransformFromCameraToWorldCoordinates(const pcl::PointXYZRGB &point, const cv::Mat &intrinsic);

		/**
		* Transform a point cloud from camera space to world space.
		*
		*/
		pcl::PointCloud<pcl::PointXYZRGB>::Ptr TransformFromCameraToWorldCoordinates(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr &sourceCloud, const cv::Mat &intrinsic);

		/**
		Transforms a point from world coordinates into camera space coordinates.
		*/
		pcl::PointXYZRGB TransformFromWordCoordinatesToCameraCoordinates(const pcl::PointXYZRGB &point, const cv::Mat &intrinsic);

		/**
		Transforms a point cloud from world coordinates into camera space coordinates.
		*/
		pcl::PointCloud<pcl::PointXYZRGB>::Ptr TransformFromWordCoordinatesToCameraCoordinates(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr &sourceCloud, const cv::Mat &intrinsic);

		/**
		Calculates the distance of a point to a plane in 3D space.
		*/
		double DistancePointPlane(const Eigen::Vector3f &point, const Eigen::Vector3f &planePoint, const Eigen::Vector3f &planeNormal);

		/**
		Performs an orthogonal projection to project a point onto a plane in 3D space.
		\param[in] point The point which should be projected.
		\param[in] planePoint A point on the plane.
		\param[in] planeNormal The normal vector of the plane.
		\return the projection of the point onto the plane.
		*/
		Eigen::Vector3f ProjectPointOnPlane(const Eigen::Vector3f &point, const Eigen::Vector3f &planePoint, const Eigen::Vector3f &planeNormal);

		/**
		Returns the squared distance of a point to an infinite long line in 3D space.
		*/
		double sqrDistancePointLine(const Vector2f &point, const Vector2f &linePoint, const Vector2f &lineDir);

		/**
		Searches the first and the last point on a line in a 3D point cloud. Every point of the point cloud is projected on the line
		and the two outermost points are returned.
		\param[in] linePointCloud The point cloud which is used to find the line subsegment.
		\param[in] linePoint A point on the line.
		\param[in] lineDirection The direction vector of the line.
		\param[out] lineBegin The first found outer point.
		\param[out] lineEnd The second found outer point.
		*/
		int FindLineStartAndEndPoint(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr &linePointCloud,
			const Eigen::Vector3f &linePoint, const Eigen::Vector3f &lineDirection,
			Eigen::Vector3f &lineBegin, Eigen::Vector3f &lineEnd);
	}

	namespace PlenopticCameraHelper
	{
		/**
		Calculates a real depth value based on the virtual depth given by the plenoptic camera.
		*/
		double CalculateRealDepthValue(double virtualDepth);

		/**
		Transforms the real depth values of a point cloud based on the virtual depth values given by the plenoptic camera.
		*/
		pcl::PointCloud<pcl::PointXYZRGB>::Ptr CalculateRealDepthValue(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr &sourceCloud);
	}
}

#endif